#include "rclcpp/rclcpp.hpp"
#include <sensor_msgs/msg/image.hpp>
#include "./detector.hpp"
#include "./PNP_Slove.hpp"
#include "engineering_interfaces/msg/target.hpp"
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include<memory>
#include"geometry_msgs/msg/transform_stamped.hpp"
#include"tf2/LinearMath/Quaternion.h"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include <Eigen/Geometry>
#include <cv_bridge/cv_bridge.h>
#include <chrono>
using namespace std::chrono_literals;


struct Transform3D{
    Eigen::Matrix3d rotation;
    Eigen::Vector3d translation;
};

class EngineeringDetectNode : public rclcpp::Node{
public:
    EngineeringDetectNode();

private:
    void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr img_msg);

    std::unique_ptr<Detector> initDetector();

    //void createDebugPublishers() noexcept;
    //void destoryDebugPublishers() noexcept;
    //Eigen::Vector3d transformPoiontToBaselink(const::Vaector)

    
    std::unique_ptr<Detector> detector_;
    std::unique_ptr<PNP_Slove>pnp_solver_;
    bool is_debug;
    rclcpp::Publisher<engineering_interfaces::msg::Target>::SharedPtr target_pub_;
    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_;
    std::shared_ptr<sensor_msgs::msg::CameraInfo> cam_info_;
    cv::Point2f cam_center_;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_sub_;
    std::shared_ptr<tf2_ros::Buffer> buffer_;
    std::shared_ptr<tf2_ros::TransformListener> listener_;
    rclcpp::TimerBase::SharedPtr timer_;
    Transform3D baselink_to_camera_;
    std::vector<std::vector<cv::Point2f>> triangles_;
    std::vector<cv::Point2f> final_targets_;
};
